/*
订阅方实现。接收雷达的坐标信息，然后创建一个点（以雷达为坐标系），转换到汽车坐标系。
关键点：
    1.创建订阅者对象
    2.坐标转换
*/
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "static_sub");
    ros::NodeHandle nh;
    //创建订阅者对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    //创建雷达扫描到的点
    geometry_msgs::PointStamped local_point;
    local_point.header.frame_id = "laser";
    local_point.point.x = 2;
    local_point.point.y = 3;
    local_point.point.z = 5;
    //设置处理点云频率
    ros::Rate rate(2);
    while (ros::ok())
    {
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(local_point, "base_link");    //坐标转换
            ROS_INFO("转换后的数据是：(%.2f,%.2f,%.2f)", point_base.point.x, point_base.point.y, point_base.point.z);
        }
        catch (const std::exception &e)
        {
            ROS_INFO("错误提示:%s.", e.what());
        }
        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}
